/**
 * Project: TacticaPescaditos
 * File: Matrix2D.java
 */
package org.javahispano.javacup.tacticas_aceptadas.Pescaditos.com.chidotronics.javacup.geometrics;

/**
 * @author Carlos Alberto
 * class: Matrix2D
 */
public class Matrix2D {

	public double _11,_12,_13;
	public double _21,_22,_23;
	public double _31,_32,_33;

	public Matrix2D() {
		this.identity();
	}

	public Vector2D toVector2D(Vector2D vPoint) {
		Vector2D result = new Vector2D();
		result.x =(_11*vPoint.x) + (_21*vPoint.y) + (_31);

		result.y  = (_12*vPoint.x) + (_22*vPoint.y) + (_32);

		return result;
	}

	public Matrix2D rotate(double rot) {
		  Matrix2D mat = new Matrix2D();

		  double sin = Math.sin(rot);
		  double cos = Math.cos(rot);
		  
		  mat._11 = cos;  mat._12 = -sin; mat._13 = 0;
		  
		  mat._21 = sin; mat._22 = cos; mat._23 = 0;
		  
		  mat._31 = 0; mat._32 = 0;mat._33 = 1;

		  return matrixMultiply(mat);
		
	}


	public void identity(){
	  this._11 = 1; this._12 = 0; this._13 = 0;
	  this._21 = 0; this._22 = 1; this._23 = 0;
	  this._31 = 0; this._32 = 0; this._33 = 1;
	  }
	

	/*
	 *		[   1    0    Tx   ]     [   cos(theta)    -sin(theta)    0   ]
     *		[   0    1    Ty   ]	 *  [   sin(theta)     cos(theta)    0   ]
     *		[   0    0    1   ]		[       0              0         1   ]
	 */
	public static Matrix2D buildToLocalSpaceMatrix(	Vector2D agentHeading,
			Vector2D agentSide,
			Vector2D agentPosition) {
		
		
		
		  double tx = -agentPosition.dot(agentHeading);
		  double ty = -agentPosition.dot(agentSide);
		Matrix2D matTransform  = new Matrix2D();
		matTransform._11 = agentHeading.x; matTransform._12 = agentSide.x;
		matTransform._21 = agentSide.y; matTransform._22 = agentHeading.y;
		matTransform._31 = tx; matTransform._32 = ty;

		return matTransform;
	}
	
	private Matrix2D matrixMultiply(Matrix2D mIn) {

		Matrix2D mat = new Matrix2D();

		mat._11 = (this._11*mIn._11) + (this._12*mIn._21) + (this._13*mIn._31);
		mat._12 = (this._11*mIn._12) + (this._12*mIn._22) + (this._13*mIn._32);
		mat._13 = (this._11*mIn._13) + (this._12*mIn._23) + (this._13*mIn._33);
		mat._21 = (this._21*mIn._11) + (this._22*mIn._21) + (this._23*mIn._31);
		mat._22 = (this._21*mIn._12) + (this._22*mIn._22) + (this._23*mIn._32);
		mat._23 = (this._21*mIn._13) + (this._22*mIn._23) + (this._23*mIn._33);
		mat._31 = (this._31*mIn._11) + (this._32*mIn._21) + (this._33*mIn._31);
		mat._32 = (this._31*mIn._12) + (this._32*mIn._22) + (this._33*mIn._32);
		mat._33 = (this._31*mIn._13) + (this._32*mIn._23) + (this._33*mIn._33);

		return mat;
	}

}
